/* 
 * Tux Racer 
 * Copyright (C) 1999-2001 Jasmin F. Patry
 * 
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 */


#include "tuxracer.h"
#include "course_load.h"
#include "course_render.h"
#include "hier.h"
#include "hier_util.h"
#include "tux.h"
#include "part_sys.h"
#include "phys_sim.h"
#include "nmrcl.h"
#include "audio.h"
#include "bonus.h"
#include "loop.h"
#ifdef TARGET_OS_IPHONE
    #include "sharedGeneralFunctions.h"
#endif
#include <stdio.h>
#include <stdlib.h>

/*
 * Constants
 */

/* Tux's mass (kg) */
#define TUX_MASS 20

/* Width of Tux (m) */
#define TUX_WIDTH 0.45

/* Minimum speed (m/s) */
#define MIN_TUX_SPEED 1.4

/* Minimum speed for friction to take effect (m/s) */
#define MIN_FRICTION_SPEED 2.8

/* Initial Tux speed (m/s) */
#define INIT_TUX_SPEED 3.0

/* Maximum frictional force */
#define MAX_FRICTIONAL_FORCE 800

/* Maximum angle by which frictional force is rotated when turning*/
#define MAX_TURN_ANGLE 45

/* Maximum turning force perpendicular to direction of movement */
#define MAX_TURN_PERPENDICULAR_FORCE 400

/* Maximum increase in friction when turning */
#define MAX_TURN_PENALTY 0.15

/* Force applied by Tux when braking (N) */
#define BRAKE_FORCE 200

/* Maximum roll angle (degrees) */
#define MAX_ROLL_ANGLE 30

/* Roll angle while braking (degrees) */
#define BRAKING_ROLL_ANGLE 55

/* Baseline friction coeff for rolling */
#define IDEAL_ROLL_FRIC_COEFF 0.35

/* Baseline speed for rolling (m/s) */
#define IDEAL_ROLL_SPEED 6.0

/* Tux's orientation is updated using a first-order filter; these are the 
 time constants of the filter when tux is on ground and airborne (s) */
#define TUX_ORIENTATION_TIME_CONSTANT 0.14
#define TUX_ORIENTATION_AIRBORNE_TIME_CONSTANT 0.5

/* Max particles generated by turning (particles/s) */
#define MAX_TURN_PARTICLES 1500

/* Max particles generated by rolling (particles/s) */
#define MAX_ROLL_PARTICLES 3000

/* Particles generated by braking (particles/s) */
#define BRAKE_PARTICLES 4000

/* Number of particles is multiplied by ( speed / PARTICLES_SPEED_FACTOR ) */
#define PARTICLE_SPEED_FACTOR 40

/* Maximum deflection angle for particles (out to side of player)  */
#define MAX_PARTICLE_ANGLE 80

/* Speed at which maximum deflection occurs */
#define MAX_PARTICLE_ANGLE_SPEED 50

/* Particle speed = min ( MAX_PARTICLE_SPEED, speed*PARTICLE_SPEED_MULTIPLIER )
 */
#define PARTICLE_SPEED_MULTIPLIER 0.3
#define MAX_PARTICLE_SPEED 2

/* The compressiblity of the first section of the spring modelling Tux's
 tush (m) */
#define TUX_GLUTE_STAGE_1_COMPRESSIBILITY 0.05

/* The spring coefficient of first section of aforementioned tush (N/m) */
#define TUX_GLUTE_STAGE_1_SPRING_COEFF 1500

/* The damping coefficient of first section of aforementioned tush (N*s/m) */
#define TUX_GLUTE_STAGE_1_DAMPING_COEFF 100

/* The compressiblity of the second section of the spring modelling Tux's
 tush (m) */
#define TUX_GLUTE_STAGE_2_COMPRESSIBILITY 0.12

/* The spring coefficient of second section of aforementioned tush (N/m) */
#define TUX_GLUTE_STAGE_2_SPRING_COEFF 3000

/* The damping coefficient of second section of aforementioned tush (N*s/m) */
#define TUX_GLUTE_STAGE_2_DAMPING_COEFF 500

/* The spring coefficient of third section of aforementioned tush (N/m) */
#define TUX_GLUTE_STAGE_3_SPRING_COEFF 10000

/* The damping coefficient of third section of aforementioned tush (N*s/m) */
#define TUX_GLUTE_STAGE_3_DAMPING_COEFF 1000

/* The maximum force exertedd by both sections of spring (N) */
#define TUX_GLUTE_MAX_SPRING_FORCE 3000

/* Maximum distance that player can penetrate into terrain (m) */
#define MAX_SURFACE_PENETRATION 0.2

/* Density of air @ -3 C (kg/m^3) */
#define AIR_DENSITY 1.308

/* Diameter of sphere used to model Tux for air resistance calcs (m) */
#define TUX_DIAMETER TUX_WIDTH

/* Viscosity of air @ -3 C (N*s/m^2) */
#define AIR_VISCOSITY 17.00e-6

/* Drag coefficient vs. Reynolds number table (from Janna, _Introduction to
 Fluid Mechanics_, 3rd Edition, PWS-Kent, p 308) 
 Values are for Re in the range of 10^-1 to 10^6, spaced logarithmically
 (all values are log base 10) */
static const double air_log_re[] = { -1,
0,
1,
2,
3,
4,
5,
6 };
static const double air_log_drag_coeff[] = { 2.25,
1.35,
0.6,
0.0,
-0.35,
-0.45,
-0.33,
-0.9 };

/* Minimum time step for ODE solver (s) */
#define MIN_TIME_STEP 0.01

/* Maximum time step for ODE solver (s) */
#define MAX_TIME_STEP 0.10

/* Maximum distance of step for ODE solver (m) */
#define MAX_STEP_DISTANCE 0.20

/* Tolerance on error in Tux's position (m) */
#define MAX_POSITION_ERROR 0.005

/* Tolerance on error in Tux's velocity (m/s) */
#define MAX_VELOCITY_ERROR 0.05

/* To smooth out the terrain, the terrain normals are interpolated
 near the edges of the triangles.  This parameter controls how much
 smoothing is done, as well as the size of the triangle boundary
 over which smoothing occurs.  A value of 0 will result in no
 smoothing, while a value of 0.5 will smooth the normals over the
 entire surface of the triangle (though the smoothing will increase
 as the value approaches infinity).  (dimensionless) */
#define NORMAL_INTERPOLATION_LIMIT 0.05

/* The distance to move Tux up by so that he doesn't sit too low on the 
 ground (m) */
#define TUX_Y_CORRECTION_ON_STOMACH 0.33

/* The square of the distance that Tux must move before we recompute a 
 collision (m^2) */
#define COLLISION_TOLERANCE 0.04

/* The square of the distance that Tux must move before we recompute a 
 collision (m^2) */
#define COLLISION_COOLDOWN_DIST 2

/* Duraction of paddling motion (s) */
#define PADDLING_DURATION 0.40

/* Force applied against ground when paddling (N) */
#define MAX_PADDLING_FORCE 122.5

/* Ideal paddling friction coefficient */
#define IDEAL_PADDLING_FRIC_COEFF 0.35

/* G force applied for jump with charge of 0 */
#define BASE_JUMP_G_FORCE 2.5

/* Maximum G force applied during jump */
#define MAX_JUMP_G_FORCE 3

/* Magnitude of force before damage is incurred (N) */
#define DAMAGE_RESISTANCE ( 4.0 * TUX_MASS * EARTH_GRAV )

/* Damage scaling factor (health/(N*s)) */
#define DAMAGE_SCALE 9e-8

/* Amount to scale tree force by (so that it does more damage than regular
 collisions) (dimensionless) */
#define COLLISION_BASE_DAMAGE 0.02

/* Damage incurred by paddling exertion (health/s) */
#define PADDLING_DAMAGE 0.02


/* Wind velocity (hard-coded for now) */
static vector_t wind_vel = { 250.0/3.6, 0, 0 };
static scalar_t wind_scale = 0.5;

/*
 * Static variables
 */

/* Friction coefficients (dimensionless) */
static scalar_t  fricCoeff[3] = { 0.35, /* ice */
0.7,  /* rock */
0.45  /* snow */
};


/* Current time step in ODE solver */
static double ode_time_step = -1;

bool_t collision_cooldown_active = False;

#ifdef TARGET_OS_IPHONE
typedef struct {
    int first_index;
    int second_index;
} range_t;
#endif

/*
 * Function definitions
 */

void increment_turn_fact( player_data_t *plyr, scalar_t amt )
{
    plyr->control.turn_fact += amt;
    plyr->control.turn_fact = min( 1.0, max( plyr->control.turn_fact, -1.0 ) );
}

scalar_t get_min_tux_speed()
{
    return MIN_TUX_SPEED;   
}

void set_friction_coeff( scalar_t fric[3] ) 
{
    fricCoeff[0] = fric[0];
    fricCoeff[1] = fric[1];
    fricCoeff[2] = fric[2];
} 

scalar_t get_min_y_coord() 
{
    scalar_t courseWidth, courseLength;
    scalar_t angle;
    scalar_t minY;
    
    get_course_dimensions( &courseWidth, &courseLength );
    angle = get_course_angle();
    
    minY = -courseLength * tan( ANGLES_TO_RADIANS( angle ) );
    return minY;
} 

void get_indices_for_point( scalar_t x, scalar_t z, 
                           int *x0, int *y0, int *x1, int *y1 )
{
    scalar_t courseWidth, courseLength;
    scalar_t xidx, yidx;
    int nx, ny;
    
    get_course_dimensions( &courseWidth, &courseLength );
    get_course_divisions( &nx, &ny );
    
    xidx = x / courseWidth * ( (scalar_t) nx - 1. );
    yidx = -z / courseLength * ( (scalar_t) ny - 1. );
    
    if (xidx < 0) {
        xidx = 0;
    } else if ( xidx > nx-1 ) {
        xidx = nx-1;
    } 
    
    if (yidx < 0) {
        yidx = 0;
    } else if ( yidx > ny-1 ) {
        yidx = ny-1;
    } 
    
    /* I found that ceil(3) was quite slow on at least some architectures, 
     so I've replace it with an approximation */
    *x0 = (int) (xidx);              /* floor(xidx) */
    *x1 = (int) ( xidx + 0.9999 );   /* ceil(xidx) */
    *y0 = (int) (yidx);              /* floor(yidx) */
    *y1 = (int) ( yidx + 0.9999 );   /* ceil(yidx) */
    
    if ( *x0 == *x1 ) {
        if ( *x1 < nx - 1 ) 
            (*x1)++;
        else 
            (*x0)--;
    } 
    
    if ( *y0 == *y1 ) {
        if ( *y1 < ny - 1 )
            (*y1)++;
        else 
            (*y0)--;
    } 
    
    check_assertion( *x0 >= 0, "invalid x0 index" );
    check_assertion( *x0 < nx, "invalid x0 index" );
    check_assertion( *x1 >= 0, "invalid x1 index" );
    check_assertion( *x1 < nx, "invalid x1 index" );
    check_assertion( *y0 >= 0, "invalid y0 index" );
    check_assertion( *y0 < ny, "invalid y0 index" );
    check_assertion( *y1 >= 0, "invalid y1 index" );
    check_assertion( *y1 < ny, "invalid y1 index" );
}

void find_barycentric_coords( scalar_t x, scalar_t z, 
                             index2d_t *idx0, 
                             index2d_t *idx1, 
                             index2d_t *idx2,
                             scalar_t *u, scalar_t *v )
{
    scalar_t xidx, yidx;
    int nx, ny;
    int x0, x1, y0, y1;
    scalar_t dx, ex, dz, ez, qx, qz, invdet; /* to calc. barycentric coords */
    scalar_t courseWidth, courseLength;
    scalar_t *elevation;
    
    elevation = get_course_elev_data();
    get_course_dimensions( &courseWidth, &courseLength );
    get_course_divisions( &nx, &ny );
    
    get_indices_for_point( x, z, &x0, &y0, &x1, &y1 );
    
    xidx = x / courseWidth * ( (scalar_t) nx - 1. );
    yidx = -z / courseLength * ( (scalar_t) ny - 1. );
    
    
    /* The terrain is meshed as follows:
     ...
     +-+-+-+-+            x<---+
     |\|/|\|/|                 |
     ...+-+-+-+-+...              V
     |/|\|/|\|                 y
     +-+-+-+-+
     ...
     
     So there are two types of squares: those like this (x0+y0 is even):
     
     +-+
     |/|
     +-+
     
     and those like this (x0+y0 is odd).
     
     +-+
     |\|
     +-+
     */
    
#define IDX(x,y) make_index2d(x,y)
    
    if ( (x0 + y0) % 2 == 0 ) {
        if ( yidx - y0 < xidx - x0 ) {
            *idx0 = IDX(x0, y0); 
            *idx1 = IDX(x1, y0); 
            *idx2 = IDX(x1, y1); 
        } else {
            *idx0 = IDX(x1, y1); 
            *idx1 = IDX(x0, y1); 
            *idx2 = IDX(x0, y0); 
        } 
    } else {
        /* x0 + y0 is odd */
        if ( yidx - y0 + xidx - x0 < 1 ) {
            *idx0 = IDX(x0, y0); 
            *idx1 = IDX(x1, y0); 
            *idx2 = IDX(x0, y1); 
        } else {
            *idx0 = IDX(x1, y1); 
            *idx1 = IDX(x0, y1); 
            *idx2 = IDX(x1, y0); 
        } 
    }
#undef IDX
    
    dx = idx0->i - idx2->i;
    dz = idx0->j - idx2->j;
    ex = idx1->i - idx2->i;
    ez = idx1->j - idx2->j;
    qx = xidx - idx2->i;
    qz = yidx - idx2->j;
    
    invdet = 1./(dx * ez - dz * ex);
    *u = (qx * ez - qz * ex) * invdet;
    *v = (qz * dx - qx * dz) * invdet;
}

vector_t find_course_normal( scalar_t x, scalar_t z )
{
    vector_t *course_nmls;
    vector_t tri_nml;
    scalar_t xidx, yidx;
    int nx, ny;
    int x0, x1, y0, y1;
    point_t p0, p1, p2;
    index2d_t idx0, idx1, idx2;
    vector_t n0, n1, n2;
    scalar_t course_width, course_length;
    scalar_t u, v;
    scalar_t min_bary, interp_factor;
    vector_t smooth_nml;
    vector_t interp_nml;
    scalar_t *elevation;
    
    elevation = get_course_elev_data();
    get_course_dimensions( &course_width, &course_length );
    get_course_divisions( &nx, &ny );
    course_nmls = get_course_normals();
    
    get_indices_for_point( x, z, &x0, &y0, &x1, &y1 );
    
    xidx = x / course_width * ( (scalar_t) nx - 1. );
    yidx = -z / course_length * ( (scalar_t) ny - 1. );
    
    find_barycentric_coords( x, z, &idx0, &idx1, &idx2, &u, &v );
    
    n0 = course_nmls[ idx0.i + nx * idx0.j ];
    n1 = course_nmls[ idx1.i + nx * idx1.j ];
    n2 = course_nmls[ idx2.i + nx * idx2.j ];
    
    p0 = COURSE_VERTEX( idx0.i, idx0.j );
    p1 = COURSE_VERTEX( idx1.i, idx1.j );
    p2 = COURSE_VERTEX( idx2.i, idx2.j );
    
    smooth_nml = add_vectors( 
                             scale_vector( u, n0 ),
                             add_vectors( scale_vector( v, n1 ), scale_vector( 1.-u-v, n2 ) ) );
    
    tri_nml = cross_product( 
                            subtract_points( p1, p0 ), subtract_points( p2, p0 ) );
    normalize_vector( &tri_nml );
    
    min_bary = min( u, min( v, 1. - u - v ) );
    interp_factor = min( min_bary / NORMAL_INTERPOLATION_LIMIT, 1.0 );
    
    interp_nml = add_vectors( 
                             scale_vector( interp_factor, tri_nml ),
                             scale_vector( 1.-interp_factor, smooth_nml ) );
    normalize_vector( &interp_nml );
    
    return interp_nml;
}

scalar_t find_y_coord( scalar_t x, scalar_t z )
{
    scalar_t ycoord;
    point_t p0, p1, p2;
    index2d_t idx0, idx1, idx2;
    scalar_t u, v;
    int nx, ny;
    scalar_t *elevation;
    scalar_t course_width, course_length;
    
    /* cache last request */
    static scalar_t last_x, last_z, last_y;
    static bool_t cache_full = False;
    
    if ( cache_full && last_x == x && last_z == z ) {
        return last_y;
    }
    
    get_course_divisions( &nx, &ny );
    get_course_dimensions( &course_width, &course_length );
    elevation = get_course_elev_data();
    
    find_barycentric_coords( x, z, &idx0, &idx1, &idx2, &u, &v );
    
    p0 = COURSE_VERTEX( idx0.i, idx0.j );
    p1 = COURSE_VERTEX( idx1.i, idx1.j );
    p2 = COURSE_VERTEX( idx2.i, idx2.j );
    
    ycoord = u * p0.y + v * p1.y + ( 1. - u - v ) * p2.y;
    
    last_x = x;
    last_z = z;
    last_y = ycoord;
    cache_full = True;
    
    return ycoord;
} 

void get_surface_type( scalar_t x, scalar_t z, scalar_t weights[] )
{
    terrain_t *terrain;
    scalar_t courseWidth, courseLength;
    int nx, ny;
    index2d_t idx0, idx1, idx2;
    scalar_t u, v;
    int i;
    
    find_barycentric_coords( x, z, &idx0, &idx1, &idx2, &u, &v );
    
    terrain = get_course_terrain_data();
    get_course_dimensions( &courseWidth, &courseLength );
    get_course_divisions( &nx, &ny );
    
    for (i=0; i<NumTerrains; i++) {
        weights[i] = 0;
        if ( terrain[ idx0.i + nx*idx0.j ] == i ) {
            weights[i] += u;
        }
        if ( terrain[ idx1.i + nx*idx1.j ] == i ) {
            weights[i] += v;
        }
        if ( terrain[ idx2.i + nx*idx2.j ] == i ) {
            weights[i] += 1.0 - u - v;
        }
    }
} 

plane_t get_local_course_plane( point_t pt )
{
    plane_t plane;
    
    pt.y = find_y_coord( pt.x, pt.z );
    
    plane.nml = find_course_normal( pt.x, pt.z );
    plane.d = -( plane.nml.x * pt.x + 
                plane.nml.y * pt.y +
                plane.nml.z * pt.z );
    
    return plane;
}

static void update_paddling( player_data_t *plyr )
{
    if ( plyr->control.is_paddling ) {
        if ( g_game.time - plyr->control.paddle_time >= PADDLING_DURATION ) {
            print_debug( DEBUG_CONTROL, "paddling off" );
            plyr->control.is_paddling = False;
        }
    }
}

void set_tux_pos( player_data_t *plyr, point_t new_pos )
{
    char *tuxRoot;
    scalar_t playWidth, playLength;
    scalar_t courseWidth, courseLength;
    scalar_t boundaryWidth;
    scalar_t disp_y;
    
    get_play_dimensions( &playWidth, &playLength );
    get_course_dimensions( &courseWidth, &courseLength );
    boundaryWidth = ( courseWidth - playWidth ) / 2; 
    
    
    if ( new_pos.x < boundaryWidth ) {
        new_pos.x = boundaryWidth;
    } else if ( new_pos.x > courseWidth - boundaryWidth ) {
        new_pos.x = courseWidth - boundaryWidth;
    } 
    
    if ( new_pos.z > 0 ) {
        new_pos.z = 0;
    } else if ( -new_pos.z >= playLength ) {
        new_pos.z = -playLength;
        set_game_mode( GAME_OVER );
    } 
    
    plyr->pos = new_pos;
    
    disp_y = new_pos.y + TUX_Y_CORRECTION_ON_STOMACH; 
    
    tuxRoot = get_tux_root_node();
    reset_scene_node( tuxRoot );
    translate_scene_node( tuxRoot, 
                         make_vector( new_pos.x, disp_y, new_pos.z ) );
} 

#ifdef TARGET_OS_IPHONE

#pragma mark trees collision

scalar_t squared_distance_to_starting_point(point_t pt) {
    point_t startingPoint;
    vector_t distVec;
    scalar_t squared_dist;
    
    point2d_t start_pt = get_start_pt();
    startingPoint.x = start_pt.x;
    startingPoint.z = start_pt.y;
    
    distVec = make_vector( pt.x - startingPoint.x, 0.0, pt.z - startingPoint.z );
    
    squared_dist = MAG_SQD(distVec);
    
    return squared_dist;
}

/* fonction utilisateur de comparaison fournie a qsort() */
static int compare_func (void const *a, void const *b)
{
    tree_t const *treeA = a;
    tree_t const *treeB = b;
    
    point_t locA,locB;
    scalar_t squared_dist_to_starting_pointA,squared_dist_to_starting_pointB;
    int ret = 0;
    
    locA = (*treeA).ray.pt;
    locB = (*treeB).ray.pt;
    
    squared_dist_to_starting_pointA = squared_distance_to_starting_point(locA);
    squared_dist_to_starting_pointB = squared_distance_to_starting_point(locB);
    
    scalar_t diff = squared_dist_to_starting_pointA - squared_dist_to_starting_pointB;
    
    if (diff > 0)
    {
        ret = 1;
    }
    else if (diff < 0)
    {
        ret = -1;
    }
    
    return ret;
}

//acces by dichotomy to first tree
int first_tree_farther_than(scalar_t squared_dist)
{
    tree_t* trees = get_tree_locs();
    int num_trees = get_num_trees();
    int top = num_trees-1;
    int bottom = 0;
    int middle=bottom+(top-bottom)/2;
    do {
        if (squared_distance_to_starting_point(trees[middle].ray.pt)<squared_dist) {
            bottom = middle;
            middle=bottom+(top-bottom)/2;
            continue;
        }
        if (squared_distance_to_starting_point(trees[middle].ray.pt)>squared_dist) {
            top = middle;
            middle=bottom+(top-bottom)/2;
            continue;
        }
    } while ((top-bottom)>1);
    
    return top;
}

//acces by dichotomy to first item
int first_item_farther_than(scalar_t squared_dist)
{
    item_t* items = get_item_locs();
    int num_items = get_num_items();
    int top = num_items-1;
    int bottom = 0;
    int middle=bottom+(top-bottom)/2;
    do {
        if (squared_distance_to_starting_point(items[middle].ray.pt)<squared_dist) {
            bottom = middle;
            middle=bottom+(top-bottom)/2;
            continue;
        }
        if (squared_distance_to_starting_point(items[middle].ray.pt)>squared_dist) {
            top = middle;
            middle=bottom+(top-bottom)/2;
            continue;
        }
    } while ((top-bottom)>1);
    
    return top;
}

range_t potential_trees_in_collision(point_t pos)
{
    /*in fact, we want to check the trees which are between two distances from the starting point.
     The difference between those two distances is (once squared) squared_diff_dist, which value was determined empirically
     with the necessity that at the end of the race, all the trees have to have been checked at least once 
     squaredDistTux, which is the distance of tuc from its starting point is in the middle of those two distances         */
    
	scalar_t squared_dist = 200.0;
    
    scalar_t squaredDistTux = squared_distance_to_starting_point(pos);
    
    int first_tree_index = first_tree_farther_than(squaredDistTux-squared_dist);
    int second_tree_index = first_tree_farther_than(squaredDistTux+squared_dist);
    
    range_t range;
    range.first_index = first_tree_index;
    range.second_index = second_tree_index;
    return range;
}

range_t potential_items_in_collision(point_t pos)
{
    /* same optimisation than for trees */
    
	scalar_t squared_dist = 200.0;
    
    scalar_t squaredDistTux = squared_distance_to_starting_point(pos);
    
    int first_item_index = first_item_farther_than(squaredDistTux-squared_dist);
    int second_item_index = first_item_farther_than(squaredDistTux+squared_dist);
    
    range_t range;
    range.first_index = first_item_index;
    range.second_index = second_item_index;
    return range;
}

/* This function order the trees in order ASC of their distance from tux starting point
 it is called just once at the begining of the race                                
 Then, instead of checking_collision with every tree of the map, with try to detect collision only with
 the trees which are ate the same distance  than tux from the starting point                      
 This should be a very important speedup                                                               */

int order_trees ()
{
    qsort (get_tree_locs(), get_num_trees(), sizeof(tree_t), compare_func);
    return 0;
}

int order_items ()
{
    qsort (get_item_locs(), get_num_items(), sizeof(item_t), compare_func);
    return 0;
}

#endif

bool_t check_tree_collisions( player_data_t *plyr, point_t pos, 
                             point_t *tree_loc, scalar_t *tree_diam )
{
    tree_t *trees;
    int num_trees, i, start;
    scalar_t diam = 0.0; 
    scalar_t height;
    polyhedron_t ph, ph2;
    matrixgl_t mat;
    point_t loc;
    bool_t hit = False;
    vector_t distvec;
    char *tux_root;
    scalar_t squared_dist;
    int tree_type;
	int speed;
    
    /* These variables are used to cache collision detection results */
    static bool_t last_collision = False;
    static point_t last_collision_tree_loc = { -999, -999, -999 };
    static scalar_t last_collision_tree_diam = 0;
    static point_t last_collision_pos = { -999, -999, -999 };
    
    /* If we haven't moved very much since the last call, we re-use
     the results of the last call (significant speed-up) */
    vector_t dist_vec = subtract_points( pos, last_collision_pos );
    if ( MAG_SQD( dist_vec ) < COLLISION_TOLERANCE ) {
        if ( last_collision ) {
            if ( tree_loc != NULL ) {
                *tree_loc = last_collision_tree_loc;
            }
            if ( tree_diam != NULL ) {
                *tree_diam = last_collision_tree_diam;
            }
            return True;
        } else {
            return False;
        }
    }
    
    trees = get_tree_locs();
    
#ifdef TARGET_OS_IPHONE
    range_t range = potential_trees_in_collision(pos);
    /*if (range.second_index-range.first_index>0) {
     printf("first : %d ; second : %d\n",range.first_index,range.second_index);
     }*/
    num_trees = range.second_index-range.first_index+1;
    assert (num_trees>=0);
    start = range.first_index;
#else
    num_trees = get_num_trees();
    start = 0;
#endif
    tree_type = trees[start].tree_type;
    ph = get_tree_polyhedron(tree_type);
    
    for (i=start; i<start+num_trees; i++) {
        diam = trees[i].diam;
        height = trees[i].height;
        loc = trees[i].ray.pt;
        
        distvec = make_vector( loc.x - pos.x, 0.0, loc.z - pos.z );
        
        /* check distance from tree; .5 is the radius of a bounding
         sphere around tux (approximate) */
        squared_dist = ( diam/2. + 0.5 );
        squared_dist *= squared_dist;
        if ( MAG_SQD( distvec ) > squared_dist ) {
            continue;
        } 
        
        /* have to look at polyhedron - switch to correct one if necessary */
        if ( tree_type != trees[i].tree_type )  {
            tree_type = trees[i].tree_type;
            ph = get_tree_polyhedron(tree_type);
        }
        
        ph2 = copy_polyhedron( ph );
        
        make_scaling_matrix( mat, diam, height, diam );
        trans_polyhedron( mat, ph2 );
        make_translation_matrix( mat, loc.x, loc.y, loc.z );
        trans_polyhedron( mat, ph2 );
        
        tux_root = get_tux_root_node();
        reset_scene_node( tux_root );
        translate_scene_node( tux_root, 
                             make_vector( pos.x, pos.y, pos.z ) );
        hit = collide( tux_root, ph2 );
        
        free_polyhedron( ph2 );
        
        if ( hit == True ) {
            if ( tree_loc != NULL ) {
                *tree_loc = loc;
            }
            if ( tree_diam != NULL ) {
                *tree_diam = diam;
            }
            
            
            break;
        }
    }

	if ( hit && MAG_SQD( subtract_points( last_collision_pos, pos ) ) > COLLISION_COOLDOWN_DIST )
	{
        last_collision = True;
		speed=(int)sqrt(plyr->vel.x*plyr->vel.x+plyr->vel.y*plyr->vel.y+plyr->vel.z*plyr->vel.z);
		set_sound_volume("hit_tree", speed<100-20 ? speed+20 : 100);
        play_sound("hit_tree",0);

        /* Record collision in player data so that health can be adjusted */
        plyr->collision = True;

	    last_collision_tree_loc = loc;
	    last_collision_tree_diam = diam;
		last_collision_pos = pos;

	}
	else
	{
        last_collision = False;
    }
    
    return hit;
} 

void check_item_collection( player_data_t *plyr, point_t pos )
{
    item_t *items;
    int num_items, i, start;
    scalar_t diam = 0.0; 
    scalar_t height;
    point_t loc;
    vector_t distvec;
    scalar_t squared_dist;
    int item_type;
    
    /* These variables are used to cache collision detection results */
    static point_t last_collision_pos = { -999, -999, -999 };
    
    /* If we haven't moved very much since the last call, we re-use
     the results of the last call (significant speed-up) */
    vector_t dist_vec = subtract_points( pos, last_collision_pos );
    if ( MAG_SQD( dist_vec ) < COLLISION_TOLERANCE ) {
        return ;
    }
    
    items = get_item_locs();
    
#ifdef TARGET_OS_IPHONE
    range_t range = potential_items_in_collision(pos);
    /*
     if (range.second_index-range.first_index>0) {
     printf("first : %d ; second : %d\n",range.first_index,range.second_index);
     }*/
    num_items = range.second_index-range.first_index+1;
    assert (num_items>=0);
    start = range.first_index;
#else
    num_items = get_num_items();
    start = 0;
#endif
    
    item_type = items[start].item_type;
    
    for (i=start; i<start+num_items; i++) {
        
        if ( items[i].collectable != 1 ) {
            continue;
#ifdef SPEED_MODE
		} else if (g_game.is_speed_only_mode) {
			break;
#endif
		} else if (!g_game.practicing) {
            break;
        }
        
        diam = items[i].diam;
        height = items[i].height;
        loc = items[i].ray.pt;
        
        distvec = make_vector( loc.x - pos.x, 0.0, loc.z - pos.z );
        
        /* check distance from item; .6 is the radius of a bounding
         sphere around tux (approximate) */
        squared_dist = ( diam/2. + 0.6 );
        squared_dist *= squared_dist;
        if ( MAG_SQD( distvec ) > squared_dist ) {
            continue;
        } 
        
        if ( (pos.y - 0.6 >= loc.y && pos.y - 0.6 <= loc.y + height) ||
            (pos.y + 0.6 >= loc.y && pos.y + 0.6 <= loc.y + height) ||
            (pos.y - 0.6 <= loc.y && pos.y + 0.6 >= loc.y + height) ) {
            /* close enough to hitting the flag */
            /* play a noise? */
            items[i].collectable = 0;
            
            plyr->herring += 1;
            add_new_bonus(Localize("Yum yum! 200 pts","src/phys_sim.c"), 0);
            play_sound( "item_collect", 0 );
        }
        
    } 
    
} 

scalar_t get_compression_depth( terrain_t surf_type ) 
{
    switch ( surf_type ) {
        case Ice:
            return 0.03;
        case Rock:
            return 0.01;
        case Snow:
            return 0.11;
        default:
            code_not_reached();
    }
    return 0;
}

/*
 * Check for tree collisions and adjust position and velocity appropriately.
 */
static void adjust_for_tree_collision( player_data_t *plyr, 
                                      point_t pos, vector_t *vel )
{
    vector_t treeNml;
    point_t treeLoc;
    bool_t treeHit;
    scalar_t speed;
    scalar_t costheta;
    scalar_t tree_diam;
    
    treeHit = check_tree_collisions( plyr, pos, &treeLoc, &tree_diam );
    if (treeHit) {
        /*
         * Calculate the normal vector to the tree; here we model the tree
         * as a vertical cylinder.
         */
        treeNml.x = pos.x - treeLoc.x;
        treeNml.y = 0;
        treeNml.z = pos.z - treeLoc.z; 
        normalize_vector( &treeNml );
        
        /* Reduce speed by a minimum of 30% */
        speed = normalize_vector( vel );
        speed *= 0.7;
        
        /* 
         * If Tux is moving into the tree, reduce the speed further, 
         * and reflect the velocity vector off of the tree
         */
        costheta = dot_product( *vel, treeNml );
        if (costheta < 0 ) {
            /* Reduce speed */
            
            speed *= 1 + costheta;
            speed *= 1 + costheta; 
            
            /* Do the reflection */
            *vel = add_vectors( 
                               scale_vector( -2. * dot_product( *vel, treeNml ) , treeNml ), 
                               *vel );
            
            normalize_vector( vel );
            
        } 
        
        speed = max( speed, get_min_tux_speed() );
        
        *vel = scale_vector( speed, *vel );
    } 
}

/*
 * Adjusts velocity so that his speed doesn't drop below the minimum 
 * speed
 */
scalar_t adjust_velocity( vector_t *vel, point_t pos, 
                         plane_t surf_plane, scalar_t dist_from_surface )
{
    vector_t surf_nml;
    scalar_t speed;
    
    surf_nml = surf_plane.nml;
    
    speed = normalize_vector( vel );
    
    if ( speed < EPS )
    {
        if ( fabs( surf_nml.x ) + fabs( surf_nml.z ) > EPS ) {
            *vel = project_into_plane( 
                                      surf_nml, make_vector( 0.0, -1.0, 0.0 ) );
            normalize_vector( vel );
        } else {
            *vel = make_vector( 0.0, 0.0, -1.0 );
        }
    }
    
    speed = max( get_min_tux_speed(), speed );
    
    *vel = scale_vector( speed, *vel );
    return speed;
}

void adjust_position( point_t *pos, plane_t surf_plane, 
                     scalar_t dist_from_surface )
{
    
    if ( dist_from_surface < -MAX_SURFACE_PENETRATION )
    {
        *pos = move_point( *pos,
                          scale_vector( -MAX_SURFACE_PENETRATION -
                                       dist_from_surface,
                                       surf_plane.nml ) );
    }
    return;
}

static vector_t adjust_tux_zvec_for_roll( player_data_t *plyr, 
                                         vector_t vel, vector_t zvec )
{
    matrixgl_t rot_mat; 
    
    vel = project_into_plane( zvec, vel );
    
    normalize_vector( &vel );
    
    if ( plyr->control.is_braking ) {
        make_rotation_about_vector_matrix( rot_mat, vel, 
                                          plyr->control.turn_fact *
                                          BRAKING_ROLL_ANGLE );
    } else {
        make_rotation_about_vector_matrix( rot_mat, vel, 
                                          plyr->control.turn_fact *
                                          MAX_ROLL_ANGLE );
    }
    
    return transform_vector( rot_mat, zvec );
}

static vector_t adjust_surf_nml_for_roll( player_data_t *plyr, 
                                         vector_t vel, 
                                         scalar_t fric_coeff,
                                         vector_t nml )
{
    matrixgl_t rot_mat; 
    scalar_t angle;
    scalar_t speed;
    scalar_t roll_angle;
    
    speed = normalize_vector( &vel );
    
    vel = project_into_plane( nml, vel );
    
    normalize_vector( &vel );
    if ( plyr->control.is_braking ) {
        roll_angle = BRAKING_ROLL_ANGLE;
    } else {
        roll_angle = MAX_ROLL_ANGLE;
    }
    
    angle = plyr->control.turn_fact *
	roll_angle *
	min( 1.0, max(0.0, fric_coeff)/IDEAL_ROLL_FRIC_COEFF )
	* min(1.0, max(0.0,speed-MIN_TUX_SPEED)/
	      (IDEAL_ROLL_SPEED-MIN_TUX_SPEED));
    
    make_rotation_about_vector_matrix( rot_mat, vel, angle );
    
    return transform_vector( rot_mat, nml );
}

#ifdef _MSC_VER
    #define INLINE
#else
    #define INLINE	inline
#endif

static INLINE scalar_t sign(scalar_t a)
{
    return a > 0 ? 1. : -1.;
}

static INLINE scalar_t absd(scalar_t a)
{
    return a > 0 ? a : -a;
}

static INLINE scalar_t jump_from_time(scalar_t t)
{
    scalar_t s = sign(t);
    t = absd(t);
    return s * (1 - exp(-5*t) + t)/(2 - exp(-5));
}

void adjust_orientation( player_data_t *plyr, scalar_t dtime, 
                        point_t pos, vector_t vel,
                        scalar_t dist_from_surface, vector_t surf_nml )
{
    vector_t new_x, new_y, new_z; 
    matrixgl_t cob_mat, inv_cob_mat;
    matrixgl_t rot_mat;
    quaternion_t new_orient;
    char* tux_root;
    scalar_t time_constant;
    static vector_t minus_z_vec = { 0., 0., -1. };
    static vector_t y_vec = { 0., 1., 0. };
    
    if ( dist_from_surface > 0 ) {
        new_y = scale_vector( 1., vel );
        normalize_vector( &new_y );
        new_z = project_into_plane( new_y, make_vector(0., -1., 0.) );
        normalize_vector( &new_z);
        new_z = adjust_tux_zvec_for_roll( plyr, vel, new_z );
    } else { 
        new_z = scale_vector( -1., surf_nml );
        new_z = adjust_tux_zvec_for_roll( plyr, vel, new_z );
        new_y = project_into_plane( surf_nml, scale_vector( 1., vel ) );
        normalize_vector(&new_y);
    }
    
    new_x = cross_product( new_y, new_z );
    
    make_change_of_basis_matrix( cob_mat, inv_cob_mat, new_x, new_y, new_z );
    
    new_orient = make_quaternion_from_matrix( cob_mat );
    
    if ( !plyr->orientation_initialized ) {
        plyr->orientation_initialized = True;
        plyr->orientation = new_orient;
    }
    
    time_constant = dist_from_surface > 0
	? TUX_ORIENTATION_AIRBORNE_TIME_CONSTANT
	: TUX_ORIENTATION_TIME_CONSTANT;
    
    plyr->orientation = interpolate_quaternions( 
                                                plyr->orientation, new_orient, 
                                                min( dtime / time_constant, 1.0 ) );
    
    plyr->plane_nml = rotate_vector( plyr->orientation, minus_z_vec );
    plyr->direction = rotate_vector( plyr->orientation, y_vec );
    
    make_matrix_from_quaternion( cob_mat, plyr->orientation );
    
    
    /* Trick rotations */
    new_y = make_vector( cob_mat[1][0], cob_mat[1][1], cob_mat[1][2] );
    if(plyr->control.barrel_roll_factor) {
        make_rotation_about_vector_matrix( rot_mat, new_y, 
                                           jump_from_time(plyr->control.barrel_roll_factor) * 360 );
        multiply_matrices( cob_mat, rot_mat, cob_mat );
    }
    new_x = make_vector( cob_mat[0][0], cob_mat[0][1], cob_mat[0][2] );
    if(plyr->control.flip_factor) {
        make_rotation_about_vector_matrix( rot_mat, new_x, 
                                       jump_from_time(plyr->control.flip_factor) * 360 );
        multiply_matrices( cob_mat, rot_mat, cob_mat );
    }
    
    transpose_matrix( cob_mat, inv_cob_mat );
    
    tux_root = get_tux_root_node();
    transform_scene_node( tux_root, cob_mat, inv_cob_mat ); 
}

scalar_t adjust_particle_count( scalar_t particles ) 
{
    if ( particles < 1 ) {
        if ( ( (scalar_t) rand() ) / RAND_MAX < particles ) {
            return 1.0;
        } else {
            return 0.0;
        }
    } else {
        return particles;
    }
}

void generate_particles( player_data_t *plyr, scalar_t dtime, 
                        point_t pos, scalar_t speed ) 
{
    point_t left_part_pt, right_part_pt;
    scalar_t brake_particles;
    scalar_t turn_particles;
    scalar_t roll_particles;
    scalar_t surf_weights[NumTerrains]; 
    scalar_t surf_y;
    scalar_t left_particles, right_particles;
    vector_t left_part_vel, right_part_vel;
    matrixgl_t rot_mat;
    vector_t xvec;
    
    get_surface_type( pos.x, pos.z, surf_weights );
    surf_y = find_y_coord( pos.x, pos.z );
    
    if ( surf_weights[Snow] > 0.5 && pos.y < surf_y ) {
        xvec = cross_product( plyr->direction, plyr->plane_nml );
        
        right_part_pt = left_part_pt = pos;
        
        right_part_pt = move_point( 
                                   right_part_pt, 
                                   scale_vector( TUX_WIDTH/2.0, xvec ) );
        
        left_part_pt = move_point( 
                                  left_part_pt, 
                                  scale_vector( -TUX_WIDTH/2.0, xvec ) );
        
        right_part_pt.y = left_part_pt.y  = surf_y;
        
        brake_particles = dtime *
	    BRAKE_PARTICLES * ( plyr->control.is_braking ? 1.0 : 0.0 )
	    * min( speed / PARTICLE_SPEED_FACTOR, 1.0 );
        turn_particles = dtime * MAX_TURN_PARTICLES 
	    * min( speed / PARTICLE_SPEED_FACTOR, 1.0 );
        roll_particles = dtime * MAX_ROLL_PARTICLES 
	    * min( speed / PARTICLE_SPEED_FACTOR, 1.0 );
        
        left_particles = turn_particles * 
	    fabs( min(plyr->control.turn_fact, 0.) ) + 
	    brake_particles +
	    roll_particles * fabs( min(plyr->control.turn_animation, 0.) );
        
        right_particles = turn_particles * 
	    fabs( max(plyr->control.turn_fact, 0.) ) + 
	    brake_particles +
	    roll_particles * fabs( max(plyr->control.turn_animation, 0.) );
        
        left_particles = adjust_particle_count( left_particles );
        right_particles = adjust_particle_count( right_particles );
        
        /* Create particle velocitites */
        make_rotation_about_vector_matrix(
                                          rot_mat, plyr->direction,
                                          max( -MAX_PARTICLE_ANGLE, 
                                              -MAX_PARTICLE_ANGLE * speed / MAX_PARTICLE_ANGLE_SPEED ) );
        left_part_vel = transform_vector( rot_mat, plyr->plane_nml );
        left_part_vel = scale_vector( min( MAX_PARTICLE_SPEED, 
                                          speed * PARTICLE_SPEED_MULTIPLIER ),
                                     left_part_vel );
        
        make_rotation_about_vector_matrix(
                                          rot_mat, plyr->direction,
                                          min( MAX_PARTICLE_ANGLE, 
                                              MAX_PARTICLE_ANGLE * speed / MAX_PARTICLE_ANGLE_SPEED ) );
        right_part_vel = transform_vector( rot_mat, plyr->plane_nml );
        right_part_vel = scale_vector( min( MAX_PARTICLE_SPEED,
                                           speed * PARTICLE_SPEED_MULTIPLIER ),
                                      right_part_vel );
        
        create_new_particles( left_part_pt, left_part_vel, 
                             (int)left_particles );
        create_new_particles( right_part_pt, right_part_vel, 
                             (int)right_particles );
    } 
}


/*
 * Calculate the magnitude of force due to air resistance (wind)
 */
vector_t calc_wind_force( vector_t player_vel )
{
    vector_t total_vel;
    scalar_t wind_speed;
    scalar_t re;         /* Reynolds number */
    scalar_t df_mag;     /* magnitude of drag force */
    scalar_t drag_coeff; /* drag coefficient */
    int table_size;      /* size of drag coeff table */
    
    static scalar_t last_time_called = -1;
    
    total_vel = scale_vector( -1, player_vel );
    
    if ( g_game.race.windy ) {
        /* adjust wind_scale with a random walk */
        if ( last_time_called != g_game.time ) {
            wind_scale = wind_scale + 
            (rand()/(scalar_t)RAND_MAX-0.50) * 0.15;
            wind_scale = min( 1.0, max( 0.0, wind_scale ) );
        }
        
        total_vel = add_vectors( total_vel, 
                                scale_vector( wind_scale, wind_vel ) );
    }
    
    wind_speed = normalize_vector( &total_vel );
    
    re = AIR_DENSITY * wind_speed * TUX_DIAMETER / AIR_VISCOSITY;
    
    table_size = sizeof(air_log_drag_coeff) / sizeof(air_log_drag_coeff[0]);
    
    drag_coeff = pow( 10.0, 
                     lin_interp( air_log_re, air_log_drag_coeff, 
                                log10(re), table_size ) );
    
    df_mag = 0.5 * drag_coeff * AIR_DENSITY * ( wind_speed * wind_speed )
	* ( M_PI * ( TUX_DIAMETER * TUX_DIAMETER ) * 0.25 );
    
    check_assertion( df_mag > 0, "Negative wind force" );
    
    last_time_called = g_game.time;
    
    return scale_vector( df_mag, total_vel );
}

static vector_t calc_spring_force( scalar_t compression, vector_t vel, 
                                  vector_t surf_nml, vector_t *unclamped_f )
{
    scalar_t spring_vel; /* velocity perp. to surface (for damping) */
    scalar_t spring_f_mag; /* magnitude of force */
    
    check_assertion( compression >= 0, 
                    "spring can't have negative compression" );
    
    spring_vel = dot_product( vel, surf_nml );
    
    /* Stage 1 */
    spring_f_mag = 
	min( compression, TUX_GLUTE_STAGE_1_COMPRESSIBILITY ) 
	* TUX_GLUTE_STAGE_1_SPRING_COEFF;
    
    /* Stage 2 */
    spring_f_mag += 
	max( 0, min( compression - TUX_GLUTE_STAGE_1_COMPRESSIBILITY, 
                TUX_GLUTE_STAGE_2_COMPRESSIBILITY ) )
	* TUX_GLUTE_STAGE_2_SPRING_COEFF;
    
    /* Stage 3 */
    spring_f_mag += 
	max( 0., compression - TUX_GLUTE_STAGE_2_COMPRESSIBILITY - 
        TUX_GLUTE_STAGE_1_COMPRESSIBILITY ) 
	* TUX_GLUTE_STAGE_3_SPRING_COEFF;
    
    /* Damping */
    spring_f_mag -= spring_vel * ( 
                                  compression <= TUX_GLUTE_STAGE_1_COMPRESSIBILITY 
                                  ? TUX_GLUTE_STAGE_1_SPRING_COEFF : 
                                  ( compression <= TUX_GLUTE_STAGE_2_COMPRESSIBILITY 
                                   ? TUX_GLUTE_STAGE_2_DAMPING_COEFF
                                   : TUX_GLUTE_STAGE_3_DAMPING_COEFF ) );
    
    /* Clamp to >= 0.0 */
    spring_f_mag = max( spring_f_mag, 0.0 );
    
    if ( unclamped_f != NULL ) {
        *unclamped_f = scale_vector( spring_f_mag, surf_nml );
    }
    
    /* Clamp to <= TUX_GLUTE_MAX_SPRING_FORCE */
    spring_f_mag = min( spring_f_mag, TUX_GLUTE_MAX_SPRING_FORCE );
    
    return scale_vector( spring_f_mag, surf_nml );
}


static vector_t calc_net_force( player_data_t *plyr, point_t pos, 
                               vector_t vel )
{
    vector_t nml_f;      /* normal force */
    vector_t unclamped_nml_f; /* unclamped normal force (for damage calc) */
    vector_t fric_f;     /* frictional force */
    scalar_t fric_f_mag; /* frictional force magnitude */
    vector_t fric_dir;   /* direction of frictional force */
    vector_t grav_f;     /* gravitational force */
    vector_t air_f;      /* air resistance force */
    vector_t brake_f;    /* braking force */
    vector_t paddling_f; /* paddling force */
    vector_t net_force;  /* the net force (sum of all other forces) */
    scalar_t comp_depth; /* depth to which the terrain can be compressed */
    scalar_t speed;      /* speed (m/s) */
    vector_t orig_surf_nml; /* normal to terrain at current position */
    vector_t surf_nml;   /* normal to terrain w/ roll effect */
    scalar_t glute_compression; /* amt that Tux's tush has been compressed */
    scalar_t steer_angle; /* Angle to rotate fricitonal force for turning */
    matrixgl_t fric_rot_mat; /* Matrix to rotate frictional force */
    vector_t jump_f;
    plane_t surf_plane;
    scalar_t dist_from_surface;
    scalar_t surf_weights[NumTerrains];
    scalar_t surf_fric_coeff;
    int i;
    
    get_surface_type( pos.x, pos.z, surf_weights );
    surf_plane = get_local_course_plane( pos );
    orig_surf_nml = surf_plane.nml;
    
    surf_fric_coeff = 0;
    for (i=0; i<NumTerrains; i++) {
        surf_fric_coeff += surf_weights[i] * fricCoeff[i];
    }
    surf_nml = adjust_surf_nml_for_roll( plyr, vel, surf_fric_coeff,
                                        orig_surf_nml );
    
    comp_depth = 0;
    for (i=0; i<NumTerrains; i++) {
        comp_depth += surf_weights[i] * get_compression_depth( (terrain_t)i );
    }
    
    
    grav_f = make_vector( 0, -EARTH_GRAV * TUX_MASS, 0 );
    
    dist_from_surface = distance_to_plane( surf_plane, pos );
    
    if ( dist_from_surface <= 0 ) {
        plyr->airborne = False;
    } else {
        plyr->airborne = True;
    }
    
    /*
     * Calculate normal force
     */
    nml_f = make_vector( 0., 0., 0. );
    if ( dist_from_surface <= -comp_depth ) {
        /*
         * Tux ended up below the surface.
         * Calculate the spring force exterted by his rear end. :-)
         */
        glute_compression = -dist_from_surface - comp_depth;
        check_assertion( glute_compression >= 0, 
                        "unexpected negative compression" );
        
        nml_f = calc_spring_force( glute_compression, vel, surf_nml,
                                  &unclamped_nml_f );
    }
    
    /* Check if player is trying to jump */
    if ( plyr->control.begin_jump == True ) {
        plyr->control.begin_jump = False;
        if ( dist_from_surface <= 0 ) {
            plyr->control.jumping = True;
            plyr->control.jump_start_time = g_game.time;
        } else {
            plyr->control.jumping = False;
        }
    }
    
    
    /* Apply jump force in up direction for JUMP_FORCE_DURATION */
    if ( ( plyr->control.jumping ) &&
        ( g_game.time - plyr->control.jump_start_time < 
         JUMP_FORCE_DURATION ) ) 
    {
        jump_f = make_vector( 
                             0, 
                             BASE_JUMP_G_FORCE * TUX_MASS * EARTH_GRAV + 
                             plyr->control.jump_amt * 
                             (MAX_JUMP_G_FORCE-BASE_JUMP_G_FORCE) * TUX_MASS * EARTH_GRAV, 
                             0 );
        
    } else {
        jump_f = make_vector( 0, 0, 0 );
        plyr->control.jumping = False;
    }
    
    /* Use the unclamped normal force for damage calculation purposes */
    plyr->normal_force = unclamped_nml_f;
    
    /* 
     * Calculate frictional force
     */
    fric_dir = vel;
    speed = normalize_vector( &fric_dir );
    fric_dir = scale_vector( -1.0, fric_dir );
    
    if ( dist_from_surface < 0 && speed > MIN_FRICTION_SPEED ) {
        vector_t tmp_nml_f = nml_f;
        
        fric_f_mag = normalize_vector( &tmp_nml_f ) * surf_fric_coeff;
        
        fric_f_mag = min( MAX_FRICTIONAL_FORCE, fric_f_mag );
        
        fric_f = scale_vector( fric_f_mag, fric_dir );
        
        
        /*
         * Adjust friction for steering
         */
        steer_angle = plyr->control.turn_fact * MAX_TURN_ANGLE;
        
        make_rotation_about_vector_matrix( fric_rot_mat, orig_surf_nml, 
                                          steer_angle );
        fric_f = transform_vector( fric_rot_mat, fric_f );
        fric_f = scale_vector( 1.0 + MAX_TURN_PENALTY, fric_f );
        
        
        /*
         * Calculate braking force
         */
        if ( speed > get_min_tux_speed() && plyr->control.is_braking ) {
            brake_f = scale_vector( 
                                   surf_fric_coeff * BRAKE_FORCE, fric_dir ); 
        } else {
            brake_f = make_vector( 0., 0., 0. );
        }
        
    } else {
        fric_f = brake_f = make_vector( 0., 0., 0. );
    }
    
    /*
     * Calculate air resistance
     */
    air_f = calc_wind_force( vel );
    
    
    /*
     * Calculate force from paddling
     */
    update_paddling( plyr );
    if ( plyr->control.is_paddling ) {
        if ( plyr->airborne ) {
            paddling_f = make_vector( 0, 0, -TUX_MASS * EARTH_GRAV / 4.0 );
            paddling_f = rotate_vector( plyr->orientation, paddling_f );
        } else {
            paddling_f = scale_vector( 
                                      -1 * min( MAX_PADDLING_FORCE,  
                                               MAX_PADDLING_FORCE * 
                                               ( MAX_PADDLING_SPEED - speed ) / MAX_PADDLING_SPEED * 
                                               min(1.0, 
                                                   surf_fric_coeff/IDEAL_PADDLING_FRIC_COEFF)) ,
                                      fric_dir );
        }
    } else {
        paddling_f = make_vector( 0., 0., 0. );
    }
    
    
    /*
     * Add all the forces 
     */
    net_force = add_vectors( jump_f, add_vectors( grav_f, 
                                                 add_vectors( nml_f, add_vectors( fric_f, 
                                                                                 add_vectors( air_f, add_vectors( brake_f, 
                                                                                                                 paddling_f ))))));
    
    return net_force;
}

static scalar_t adjust_time_step_size( scalar_t h, vector_t vel )
{
    scalar_t speed;
    
    speed = normalize_vector( &vel );
    
    h = max( h, MIN_TIME_STEP );
    h = min( h, MAX_STEP_DISTANCE / speed );
    h = min( h, MAX_TIME_STEP );
    
    return h;
}

/*
 * Solves the system of ordinary differential equations governing Tux's 
 * movement.  Based on Matlab's ode23.m, (c) The MathWorks, Inc.
 */
void solve_ode_system( player_data_t *plyr, scalar_t dtime ) 
{
    double t0, t, tfinal;
    ode_data_t *x, *y, *z, *vx, *vy, *vz; /* store estimates of derivs */
    ode_solver_t solver;
    double h;
    bool_t done = False;
    bool_t failed = False;
    point_t new_pos;
    vector_t new_vel, tmp_vel;
    scalar_t speed;
    vector_t new_f;
    point_t saved_pos;
    vector_t saved_vel, saved_f;
    double pos_err[3], vel_err[3], tot_pos_err, tot_vel_err;
    double err=0, tol=0;
    int i;
    
    /* Select a solver */
    switch ( getparam_ode_solver() ) {
        case EULER:
            solver = new_euler_solver();
            break;
        case ODE23:
            solver = new_ode23_solver();
            break;
        case ODE45:
            solver = new_ode45_solver();
            break;
        default:
            setparam_ode_solver( ODE23 );
            solver = new_ode23_solver();
    }
    
    /* Select an initial time step */
    h = ode_time_step;
    
    if ( h < 0 || solver.estimate_error == NULL ) {
        h = adjust_time_step_size( dtime, plyr->vel );
    }
    
    t0 = 0;
    tfinal = dtime;
    
    t = t0;
    
    /* Create variables to store derivative estimates & other data */
    x  = solver.new_ode_data();
    y  = solver.new_ode_data();
    z  = solver.new_ode_data();
    vx = solver.new_ode_data();
    vy = solver.new_ode_data();
    vz = solver.new_ode_data();
    
    /* initialize state */
    new_pos = plyr->pos;
    new_vel = plyr->vel;
    new_f   = plyr->net_force;
    
    /* loop until we've integrated from t0 to tfinal */
    while (!done) {
        
        if ( t >= tfinal ) {
            print_warning( CRITICAL_WARNING, 
                          "t >= tfinal in solve_ode_system()" );
            break;
        }
        
        /* extend h by up to 10% to reach tfinal */
        if ( 1.1 * h > tfinal - t ) {
            h = tfinal-t;
            check_assertion( h >= 0., "integrated past tfinal" );
            done = True;
        }
        
        print_debug( DEBUG_ODE, "h: %g", h );
        
        saved_pos = new_pos;
        saved_vel = new_vel;
        saved_f = new_f;
        
        /* Loop until error is acceptable */
        failed = False;
        
        for (;;) {
            
            /* Store initial conditions */
            solver.init_ode_data( x, new_pos.x, h );
            solver.init_ode_data( y, new_pos.y, h );
            solver.init_ode_data( z, new_pos.z, h );
            solver.init_ode_data( vx, new_vel.x, h );
            solver.init_ode_data( vy, new_vel.y, h );
            solver.init_ode_data( vz, new_vel.z, h );
            
            
            /* We assume that the first estimate in all ODE solvers is equal 
             to the final value of the last iteration */
            solver.update_estimate( x, 0, new_vel.x );
            solver.update_estimate( y, 0, new_vel.y );
            solver.update_estimate( z, 0, new_vel.z );
            solver.update_estimate( vx, 0, new_f.x / TUX_MASS );
            solver.update_estimate( vy, 0, new_f.y / TUX_MASS );
            solver.update_estimate( vz, 0, new_f.z / TUX_MASS );
            
            /* Update remaining estimates */
            for ( i=1; i < solver.num_estimates(); i++ ) {
                new_pos.x = solver.next_val( x, i );
                new_pos.y = solver.next_val( y, i );
                new_pos.z = solver.next_val( z, i );
                new_vel.x = solver.next_val( vx, i );
                new_vel.y = solver.next_val( vy, i );
                new_vel.z = solver.next_val( vz, i );
                
                solver.update_estimate( x, i, new_vel.x );
                solver.update_estimate( y, i, new_vel.y );
                solver.update_estimate( z, i, new_vel.z );
                
                new_f = calc_net_force( plyr, new_pos, new_vel );
                
                solver.update_estimate( vx, i, new_f.x / TUX_MASS );
                solver.update_estimate( vy, i, new_f.y / TUX_MASS );
                solver.update_estimate( vz, i, new_f.z / TUX_MASS );
            }
            
            /* Get final values */
            new_pos.x = solver.final_estimate( x );
            new_pos.y = solver.final_estimate( y );
            new_pos.z = solver.final_estimate( z );
            
            new_vel.x = solver.final_estimate( vx );
            new_vel.y = solver.final_estimate( vy );
            new_vel.z = solver.final_estimate( vz );
            
            /* If the current solver can provide error estimates, update h
             based on the error, and re-evaluate this step if the error is 
             too large  */
            if ( solver.estimate_error != NULL ) {
                
                /* Calculate the error */
                pos_err[0] = solver.estimate_error( x );
                pos_err[1] = solver.estimate_error( y );
                pos_err[2] = solver.estimate_error( z );
                
                vel_err[0] = solver.estimate_error( vx );
                vel_err[1] = solver.estimate_error( vy );
                vel_err[2] = solver.estimate_error( vz );
                
                tot_pos_err = 0.;
                tot_vel_err = 0.;
                for ( i=0; i<3; i++ ) {
                    pos_err[i] *= pos_err[i];
                    tot_pos_err += pos_err[i];
                    vel_err[i] *= vel_err[i];
                    tot_vel_err += vel_err[i];
                }
                tot_pos_err = sqrt( tot_pos_err );
                tot_vel_err = sqrt( tot_vel_err );
                
                print_debug( DEBUG_ODE, "pos_err: %g, vel_err: %g", 
                            tot_pos_err, tot_vel_err );
                
                if ( tot_pos_err / MAX_POSITION_ERROR >
                    tot_vel_err / MAX_VELOCITY_ERROR )
                {
                    err = tot_pos_err;
                    tol = MAX_POSITION_ERROR;
                } else {
                    err = tot_vel_err;
                    tol = MAX_VELOCITY_ERROR;
                }
                
                /* Update h based on error */
                if (  err > tol  && h > MIN_TIME_STEP + EPS  )
                {
                    done = False;
                    if ( !failed ) {
                        failed = True;
                        h *=  max( 0.5, 0.8 *
                                  pow( tol/err, 
                                      solver.time_step_exponent() ) );
                    } else {
                        h *= 0.5;
                    }
                    
                    h = adjust_time_step_size( h, saved_vel );
                    
                    new_pos = saved_pos;
                    new_vel = saved_vel;
                    new_f = saved_f;
                    
                } else {
                    /* Error is acceptable or h is at its minimum; stop */
                    break;
                }
                
            } else {
                /* Current solver doesn't provide error estimates; stop */
                break;
            }
        }
        
        /* Update time */
        t = t + h;
        
        tmp_vel = new_vel;
        speed = normalize_vector( &tmp_vel );
        
        /* only generate particles if we're drawing them */
        if ( getparam_draw_particles() ) {
            generate_particles( plyr, h, new_pos, speed );
        }
        
        /* Calculate the final net force */
        new_f = calc_net_force( plyr, new_pos, new_vel );
        
        /* If no failures, compute a new h */
        if ( !failed && solver.estimate_error != NULL ) {
            double temp = 1.25 * pow(err / tol, solver.time_step_exponent());
            if ( temp > 0.2 ) {
                h = h / temp;
            } else {
                h = 5.0 * h;
            }
        }
        
        /* Clamp h to constraints */
        h = adjust_time_step_size( h, new_vel );
        
        /* Important: to make trees "solid", we must manipulate the 
         velocity here; if we don't and Tux is moving very quickly,
         he can pass through trees */
        adjust_for_tree_collision( plyr, new_pos, &new_vel );
        
        /* Try to collect items here */
#ifdef SPEED_MODE
        if (!g_game.is_speed_only_mode)
#endif
            check_item_collection( plyr, new_pos );        
    }
    
    /* Save time step for next time */
    ode_time_step = h;
    
    plyr->vel = new_vel;
    plyr->pos = new_pos;
    plyr->net_force = new_f;
    
    free( x );
    free( y );
    free( z );
    free( vx );
    free( vy );
    free( vz );
}


/* 
 * Updates Tux's position taking into account gravity, friction, tree 
 * collisions, etc.  This is the main physics function.
 */
void update_player_pos( player_data_t *plyr, scalar_t dtime )
{
    vector_t surf_nml;   /* normal vector of terrain */
    vector_t tmp_vel;
    scalar_t speed;
    scalar_t paddling_factor; 
    vector_t local_force;
    scalar_t flap_factor;
    plane_t surf_plane;
    scalar_t dist_from_surface;
    
    if ( dtime > 2. * EPS ) {
        solve_ode_system( plyr, dtime );
    }
    
    tmp_vel = plyr->vel;
    
    /*
     * Set position, orientation, generate particles
     */
    surf_plane = get_local_course_plane( plyr->pos );
    surf_nml = surf_plane.nml;
    dist_from_surface = distance_to_plane( surf_plane, plyr->pos );
    adjust_velocity( &plyr->vel, plyr->pos, surf_plane, 
                    dist_from_surface );
    adjust_position( &plyr->pos, surf_plane, dist_from_surface );
    
    speed = normalize_vector( &tmp_vel );
    
    set_tux_pos( plyr, plyr->pos );
    adjust_orientation( plyr, dtime, plyr->pos, plyr->vel, 
                       dist_from_surface, surf_nml );
    
    flap_factor = 0;
    
    if ( plyr->control.is_paddling ) {
        scalar_t factor;
        factor = (g_game.time - plyr->control.paddle_time) / PADDLING_DURATION;
        if ( plyr->airborne ) {
            paddling_factor = 0;
            flap_factor = factor;
        } else {
            paddling_factor = factor;
            flap_factor = 0;
        }
    } else {
        paddling_factor = 0.0;
    }
    
    /* calculate force in Tux's local coordinate system */
    local_force = rotate_vector( quaternion_conjugate( plyr->orientation ),
                                plyr->net_force );
    
    if (plyr->control.jumping) {
        flap_factor = (g_game.time - plyr->control.jump_start_time) / 
	    JUMP_FORCE_DURATION;
    } 
    
    adjust_tux_joints( plyr->control.turn_animation, plyr->control.is_braking,
                      paddling_factor, speed, local_force, flap_factor );
}

void init_physical_simulation()
{
    int i;
    player_data_t *plyr;
    for ( i=0; i<g_game.num_players; i++ ) {
		scalar_t ycoord;
		point_t p;
        plyr = get_player_data( i );
        ycoord = find_y_coord( plyr->pos.x, plyr->pos.z );
        p = make_point(plyr->pos.x, ycoord ,plyr->pos.z);
        init_physical_simulation_at_point(plyr,p);
    }
}

void init_physical_simulation_at_point(player_data_t *plyr, point_t point)
{
    vector_t nml;
    matrixgl_t rotMat;
    vector_t init_vel;
    vector_t init_f;
    
    plyr->pos.x=point.x;
    plyr->pos.y=point.y;
    plyr->pos.z=point.z;
    
    nml = find_course_normal(plyr->pos.x, plyr->pos.z );
    make_rotation_matrix( rotMat, -90., 'x' );
    init_vel = transform_vector( rotMat, nml );
    init_vel = scale_vector( INIT_TUX_SPEED, init_vel );
    init_f = make_vector( 0., 0., 0. );

    plyr->vel = init_vel;
    plyr->net_force = init_f;
    plyr->control.turn_fact = 0.0;
    plyr->control.turn_animation = 0.0;
    plyr->control.barrel_roll_factor = 0.0;
    plyr->control.flip_factor = 0.0;
    plyr->control.is_braking = False;
    plyr->orientation_initialized = False;
    plyr->plane_nml = nml;
    plyr->direction = init_vel;
    plyr->normal_force = make_vector(0,0,0);
    plyr->airborne = False;
    plyr->collision = False;
    plyr->control.jump_amt = 0;
    plyr->control.is_paddling = False;
    plyr->control.jumping = False;
    plyr->control.jump_charging = False;
    plyr->control.barrel_roll_left = False;
    plyr->control.barrel_roll_right = False;
    plyr->control.barrel_roll_factor = 0;
    plyr->control.front_flip = False;
    plyr->control.back_flip = False;
#ifdef TARGET_OS_IPHONE
    order_trees();
    order_items();
#endif
    
    ode_time_step = -1;
} 


